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Abstract 

The use of active compliance enables robots to carry out tasks in the presence 
of significant sensing and control errors. Compliant motions are quite difficult for 
humans to specify, however. Furthermore, robot programs are quite sensitive to 
details of geometry and to error characteristics and must, therefore, be constructed 
anew for each task. These factors motivate the need for automatic synthesis tools 
for robot programming, especially for compliant motion. This paper describes a 
formal approach to the synthesis of compliant motion strategies from geometric 
descriptions of assembly operations and explicit estimates of errors in sensing and 
control. A key aspect of the approach is that it provides correctness criteria for 
compliant motion strategies. 
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1. Introduction 


The central robot programming problem lies in achieving tasks in spite of 
uncertainty in the robot’s position relative to external objects. The use of sensing 
to reduce uncertainty significantly extends the range of possible tasks. Sensor-based 
robot programs are very difficult to write, however, as there is little theory to serve 
as a guide. To make matters worse, programs written for one task are seldom, 
if ever, applicable to other tasks. These two points make the development of an 
automatic synthesis strategy for sensor-based robot programs a key priority. 

In this paper, we propose a formal approach to the automatic synthesis of a 
class of compliant fine-motion strategies applicable to assembly tasks. The approach 
uses geometric descriptions of parts and estimates of measurement and motion 
errors to produce fine-motion strategies. Although our description of the approach 
will be in the form of an abstract algorithm, no implementation of this approach 
exists at present (although implementation is in progress). The formalism provides 
a structured way of thinking about fine-motion strategies and, therefore, may be 
helpful to human programmers of fine-motion strategies. 

1.1. Fine-motion strategies 

One important source of the difficulty in robot programming is that the 
programmer’s model of the environment is incomplete and inexact as to the shape 
and location of objects. Vision may be used to determine the approximate shape 
and positions of objects, but generally not with sufficient accuracy for assembly 
by pure position control. Knowing the object shapes and positions to sufficient 
accuracy is not enough, however. Positioning errors inherently limit the tasks 
achievable by strict position control. Increasing the mechanical accuracy of robots 
to the levels required for assembly is expensive and ultimately stifling. Instead, one 
must abandon the paradigm of pure position control for tasks where the allowable 
motions are tightly constrained by external objects, as they are in mechanical 
assembly. 

The basic method for achieving constrained motion in the presence of position 
uncertainty is by the use of controlled compliance (see [Mason 83] for an overview 
of compliance research). Compliant motion meets external constraints by specifying 
how the robot’s motion should be modified in response to the forces generated 
when the constraints are violated. Contact with a surface, for example, can be 
guaranteed by moving so that a small force normal to the surface is maintained. 
Using this technique, the robot can achieve and retain contact with a surface that 
may vary significantly in shape and orientation from the programmer’s expectations. 
Generalizations of this principle can be used to accomplish a wide variety of tasks 
involving constrained motion, e.g., inserting a peg in a hole and following a weld 
seam. 

The specification of particular compliant motions to achieve a task requires 
knowledge of the geometric constraints imposed by the task. Given a description of 
the constraints, choices can be made for the compliant motion parameters, e.g., the 
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Figure 1. Some possible initial configurations for peg-in-hole insertion 



motion freedoms to be force controlled and those to be position controlled [Mason 
81, Paul and Shimano 76, Raibert and Craig 81], or the center of compliance and 
axis stiffnesses [Hanafusa and Asada 77, Salisbury 80, Whitney 83]. It is common, 
however, for position uncertainty to be large enough so that the programmer cannot 
unambiguously determine which geometric constraint holds at any instant in time. 
Figure 1, for example, shows some different initial, conditions that can hold in 
two-dimensional peg-in-hole insertion. Under these circumstances, the programmer 
must employ a combined strategy of force and position control that guarantees 
reaching the desired final configuration from all of the likely initial configurations. 
We call such a strategy a fine-motion strategy. 

One of the most widely studied tasks in robotics is the two-dimensional 
peg-in-hole task. Detailed analyses have been carried out to determine strategies 
that guarantee successful insertion once the peg is partly in the hole [Drake 77, 
McCallion and Wong 75, Ohwovoriole and Roth 81, Simunovic 75, Whitney 82]. 
When the initial uncertainty in position is large enough, a strategy must also be 
devised to ensure that the peg can find the hole [Inoue 74, McCallion and Wong 
75]. We can illustrate a variety of strategies for one task by considering the ways 
this problem has been addressed: 

1. Chamfers: Chamfers on the hole entrance and/or the peg tip increase 
the range of relative positions where the peg can fall into the hole, at 
least partway. This technique is especially effective if the peg support has 
lateral compliance [Drake 77, Whitney 82]. 

2. Tilting the peg: Tilting the peg slightly also increases the range of relative 
positions where initial entry into the hole is guaranteed [Inoue 74]. In fact, 
the geometric effect of tilting the peg is almost identical to providing a 
chamfer (see Section 2.6). 
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3. Search: The simplest strategy is a search by sliding along the top surface 
until the peg falls into the hole. In general, the search will have to pick an 
initial direction of motion and, possibly, back up if the hole is not found. 

4. Biased Search: A slight modification to the search strategy is to introduce 
a bias into the initial position of the peg [Inoue 74]. This strategy reduces 
the chances of initial entry into the hole, but it guarantees that the peg 
will be to one side of the hole. 

In this paper we will be interested in strategies such as tilting the peg and 
biased search. These are simple strategies employing compliant motion that do not 
require modifying the task geometry or complicated control structures. 

1.2. Previous work 

In this paper we present an approach to the automatic synthesis of a class 
of fine-motion strategies. We are aware of no previous work with the same goal. 
There are, however, several bodies of work relevant to this goal. The first of these 
deals with analyses of geometry and statics of tasks so as to develop conditions 
that successful fine-motion strategies must satisfy. The second is Simunovic’s 
“information approach”. The third group deals with attempts to derive strategies 
starting from partially specified strategies, known as skeletons or plans. The fourth 
group deals with attempts to have the robot “learn” strategies from experience and 
partial task information. 

Quite a few authors have analyzed the peg-in-hole assembly task in detail 
[Drake 77, Laktionev and Andreev 66, Andreev and Laktionev 69, Gusev 69, 
MeCallion and Wong 75, Ohwovoriole, Roth, and Hill 81, Ohwovoriole and Roth 
81, Simunovic 75, Whitney 82]. In most of the analyses, the assumption is that 
the peg is initially partly in the hole, possibly at a chamfer. Two important failure 
modes during insertion have been identified: jamming and wedging. Jamming is 
due to misproportioned applied forces; wedging is due to geometric conditions that 
arise when the parts deform slightly. These analyses have led to the formulation of 
conditions for successful insertion involving applied forces to relative positions of the 
peg and hole. As a result, a mechanical device (called the RCC [Drake 75, Whitney 
82]) has been built that applies the correct forces in response to small initial errors 
between the peg and hole. A number of heuristic strategies for peg-in-hole insertion 
have also been formulated, based on more fragmentary analysis. These heuristic 
strategies have been used successfully in practice [Inoue 74, Goto, Takeyasu, and 
Inoyama 81]. 

Mason’s [82] detailed analysis of pushing and grasping operations in the 
presence of friction also leads to conditions for successful task completion. These 
conditions provide the basis for synthesis of operations that succeed in the presence 
of uncertainty (without requiring sensing). 

Simunovic [79] formulated the “information approach” to fine-motion based 
on the principle that assembly is purely a relative positioning task. Prom this 
premise he argues that the role of an assembly program is to determine the relative 
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positions of parts during an assembly and to issue position commands to correct the 
errors. He developed an estimation technique to infer, from a series of noisy position 
measurements and using knowledge of the geometry of the parts, the actual relative 
positions of the parts. One problem with this approach is that it requires a very 
large amount of on-line computation, although this could be solved with special 
purpose electronics. A more fundamental problem is that the approach assumes 
only position control and a robot capable of making fine incremental motions. This 
need not be the case for assembly; by exploiting compliant behavior the robot can 
achieve high accuracy tasks even with low accuracy position control, for example 
following a surface by maintaining a downward force. Another problem is that 
Simunovic’s estimation technique requires knowing which surfaces are in contact. 
This limits the method to situations with relatively small errors; in more general 
cases, the identity of the contact surfaces will not be known. Our approach is based 
on a different view of assembly: that the geometric constraints should “guide” the 
parts to their destination without necessarily having to know exactly where the 
parts are relative to each other. 

One of the earliest explorations in the area of automatic synthesis of fine- 
motion strategies from strategy skeletons was by Taylor [76]. Taylor developed a 
technique for propagating the effect of errors and uncertainties through a model of 
a task. These error estimates were used to make decisions for filling-in the strategy 
skeletons. For peg-in-hole insertion, for example, the decision whether to tap the 
peg against the surface next to the hole was based on whether the error estimate 
for position normal to the surface exceeded a threshold. 

Lozano-Perez [76] also proposed a method for selecting the motion parameters 
in strategy skeletons. Each motion in a skeleton was specified symbolically by the 
relationship among parts that it was designed to achieve. The expected length of 
guarded moves and their force terminating conditions were then computed from 
the ranges of displacements that achieved this relationship (taking into account 
uncertainty in position). 

Recently, Brooks [82] extended Taylor’s approach by making more complete 
use of symbolic constraints in the error computations. The resulting constraints can 
be used in the “forward” direction to estimate errors for particular operations. But, 
importantly, they also may be used in the “backward” direction to constrain the 
values for plan parameters, such as initial positions of objects, to those that enable 
the plan to succeed. When no good choice of parameters exists, the system chooses 
appropriate sensing operations (such as visual location of parts) that reduce the 
uncertainty enough to guarantee success. 

Another line of research has focused on building up programs automatically 
from attempts by the robot to carry out the operations. Dufay and Latombe [83] 
describe how partial local strategies (“rules”) for a task can be assembled into a 
complete program by processing the execution traces of many attempts to carry out 
the task. The method, however, requires knowing the actual relationship between 
parts achieved by each motion, e.g., which surfaces are in contact. This information 
can be obtained, in many cases, from careful analysis of the forces and positions 
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Figure 2. Variations of peg-in-hole require different strategies 
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but, in general, the information is ambiguous in the presence of measurement and 
control errors. Moreover, the rules used by the system are specific to tasks and 
must be provided by the user. 


A related approach to deriving a strategy from “experiments” is based on the 
theory of stochastic automata [Simons, et al. 82]. The goal is to have the robot learn 
the appropriate control response to measured force vectors during task execution. 
The method requires a task-dependent evaluation function so as to judge progress 
towards its goal. 

These previous approaches to fine-motion synthesis are based on the assumption 
that there is a basic repertoire of operations, such as peg-in-hole insertion and 
block-in-corner, whose geometric structure is known a priori. In this view, the task 
of a synthesis program is to make some pre-defined set of choices among alternative 
actions, select the values of some parameters, and, possibly, select the order of 
operations. In fact, small changes in the geometry of parts can have significant 
impact on fine-motion strategies. The different operations shown in Figure 2, for 
example, can all be classified as peg-in-hole and, yet, they require substantially 
different programs to insure reliable execution. Similarly, differences in expected 
position errors will call for different strategies for the same task. 

Our approach is motivated by the belief that the set of possible geometric 
interactions in a task should directly determine the structure of the fine-motion 
strategy for the task. Thus, for example, the presence of additional surfaces within 
the region of possible initial contact typically requires a change in the structure of a 
strategy. The approach we describe in this paper proceeds directly from geometric 
descriptions of the parts to a strategy. 





Automatic Synthesis of 


Fine-Motion Strategies for Robots 


Figure 3. Peg-in-hole: (a) original formulation (b) transformed to point problem 



2. Overview of the approach 

In this section we informally outline our approach to fine-motion synthesis 
using a progression of simple examples. In section 3, we provide a more formal 
characterization of the approach. 


2.1. The basic strategy 

Consider the simple task of moving the point p from its initial position to 
any one of the positions in G (see Figure 3(b)). This is a simplified problem but 
not a completely artificial one. It is equivalent to the two-dimensional peg-in-hole 
problem in Figure 3(a) when the axes of the peg and hole are constrained to be 
parallel. The position of p determines the position of the peg. The boundary of 
the shaded area represents the positions of p where the peg would be in contact 
with an obstacle. The transformation from Figure 3(a) to Figure 3(b) corresponds 
to shrinking the peg to a point and expanding the obstacles accordingly. Note that 
the sides of the hole have each been moved towards each other by half the width 
of the peg. In this case, the transformation produces an equivalent problem. We 
postpone a more general discussion of this type of transformation until section 2.6. 
It is the case that problems of moving rigid objects among other rigid objects can be 
reformulated as equivalent problems of moving a point among transformed objects 
in a higher dimensional space, called the configuration space [Lozano-Perez 81, 
83]. 

The basic step in our synthesis approach is to identify ranges of positions from 
where p can reach G by a single motion. The directions of such motions can be 
represented as unit velocity vectors, v,-. For each v;, we can compute all those 
positions, Pi, such that a motion along v; from that position would reach some 
point of G (see Figure 4). We call this range of positions that can reach the goal 
by a single motion along a specified velocity the pre-image 1 of the goal (for that 
velocity). All we need do to guarantee that p reaches G from any point in any of 
the Pi is to execute a motion with commanded velocity along v*. 

lr The rationale for this name stems from viewing motions as mappings from pairs of initial 
positions and velocities into points along the resulting path. 


6 





Automatic Synthesis of 


Fine-Motion Strategies for Robots 


Figure 4. Pre-image of the goal for different v^. 
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If no pre-image of G contains the peg’s current position, then we can apply 
the same pre-image computation recursively using each of the existing pre-images 
as a possible goal. This recursive process is an instance of the problem solving 
strategy known as backward, chaining [Nilsson 80]. Each pre-image of G, Pi, serves 
to define a new goal set G\ (the superscript indicates the “recursion level”). This 
process is repeated until some pre-image P* contains the current position of p (see 
Figure 5). From this chain of pre-images Pi 0 ,P } l ..., P\ k we can construct a motion 
strategy. The two components of the strategy are a sequence of velocity vectors and 
a sequence of associated termination predicates. Therefore, the strategies may be 
construed as a sequence of guarded motions [Will and Grossman 75]. Each velocity 
vector Yi. defines a motion that moves from anywhere in P\. to G\ _ x . Whenever p 
reaches one of the goal regions, the velocity command needs to be changed to that 
appropriate for the new region, e.g., from v t \ to Vj 3 ._j. The role of the termination 
predicates is to detect the arrival of p into a goal region. In the simple case we have 
been discussing, termination predicates simply test to see whether the position is 
in the goal region. Termination predicates are much more difficult to construct in 
the presence of position uncertainty. We will discuss this issue further in section 
2.2 and section 3. 

In summary, our basic approach to fine-motion synthesis is to chain backwards 
from the goal towards the current position, characterizing at each step the range 
of positions that can reach the current goal in one motion, i.e., the pre-image 
of the goal. It remains to show how this simple approach is applicable to more 
realistic assembly problems. Our first step towards this goal will be to discuss the 
role played by uncertainty in position and velocity. The second step will be to 
introduce compliant motions. The next step will be to illustrate how friction can 
be handled. The last step is to show how the notion of configuration space reduces 
assembly problems for solids into problems involving a point and surfaces in a 
higher-dimensional space. 

2.2. The effect of uncertainty 

We have assumed thus far that p’s position is known exactly at all times and 
that its direction of motion can be specified exactly. In this section we explore the 
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Figure 5. Backward chaining of pre-images 




Figure 6. Weak and strong pre-images of a goal under velocity uncertainty 




effects of relaxing these assumptions. 

Let us assume that there is error between the actual and the commanded 
velocity, bounded by e v . The actual velocity is within a ball of radius e„ in velocity 
space (the ball of velocities centered on v is denoted B(v)). Therefore, the path 
of p is constrained to be within a semi-infinite cone centered on the commanded 
path and whose apex is the initial position. The angle between the actual direction 
of motion and the'commanded direction is constrained to be less than or equal to 
sin -1 e v> which will be approximately e v for small enough e„. 

The synthesis approach above is based on computing the pre-images of goal 
regions for particular values of commanded velocity. These are locations from which 
the goal can be reached by a single motion. In the presence of uncertainty in the 
actual velocity, we define two alternative pre-images (see Figure 6): 

1. Weak pre-image - locations for which some motion within the range of 
velocity uncertainty may reach the goal. 

2. Strong pre-image - locations for which all motions within the range of 
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Figure 7. Positions reachable by commanded motion with uncertainty 
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velocity uncertainty will reach the goal. 

Note that the strong pre-image is a subset of the weak pre-image. In what 
follows, we will use the term pre-image to mean strong pre-image. 

In addition to uncertainty in the actual velocity along a motion, there is 
uncertainty in the position of p. One source of position uncertainty is due to 
imperfect knowledge of the initial position of the objects in the workspace. Another 
source of error is due to inherent limitations in the robot’s position sensors. For the 
sake of simplicity, we will lump these two types of uncertainty into a single upper 
bound on position uncertainty. This assumption does not affect correctness of any 
derived motion strategies, but might lead to less efficient strategies. In practice, the 
two sources of uncertainty should be treated differently. 

We assume that the actual position is always within a ball of radius e c centered 
at the position observed by the robot. All possible observed positions are within a 
similar ball centered at the actual position. The ball of possible observed positions 
centered at a position p is denoted B(p). The range of positions potentially traversed 
by a motion from an observed position along a commanded velocity is depicted in 
Figure 7. 

Position uncertainty makes it difficult to define termination predicates for 
motions. A predicate that simply compares the observed position of p against the 
boundaries of G l could terminate a motion prematurely. The actual position of p 
could be anywhere within a ball of radius e c from the observed position. In order to 
guarantee success all possible positions of p must be within the goal. We can think 
of this effect of position uncertainty as “shrinking” the goal by e c for purposes of 
detecting entry. Shrinking G 1 removes from G l any point at a distance less than 
or equal to e c from any point in free space not in G l . This removes from G 1 any 
point that is ambiguous. In many cases, this means that no part of the goal is 
unambiguously identifiable on the basis of position. We will have to rely on the 
effects of collisions with surfaces or on previous history to identify entry into a goal 
region. This issue is quite subtle; it is the subject of section 3.1 (also see section 
2.5). 
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^ 2.3. Compliant Motion 

The example above dealt only with position controlled motions. Due to 
uncertainty in p’s position and velocity relative to the task, this type of motion 
often leads to empty pre-images. This indicates that the position accuracy is not 
sufficient for the task. We mentioned earlier that the alternative motion regime is 
compliant motion. We can visualize the effect of compliant motions as producing 
sliding on the constraint surfaces derived from the obstacles. Sliding means that 
the moving object confines its motions to be tangent to the constraining surface(s) 
[Mason 81]. When not in contact with a surface, the motion will be along the 
commanded velocity (to within the velocity uncertainty). 

The generalized damping model [Whitney 76] can be used to implement 
compliant motions with the properties described above. The desired motion is 
determined by the following relationship 

f = B(v — vo) 

where f is the vector of forces acting on the moving object, vo is the nominal 
velocity vector, and v is the actual velocity vector. In what follows, B is a diagonal 
matrix. The role of B in our usage is primarily to relate the units of force to those of 
velocity. We assume that the control system ensures that the dynamics of the robot 
and moving object can be adequately approximated as a damper (for some limited 
range of operating velocities). The Appendix provides a more detailed treatment of 
/•-v the behavior of a generalized damper. 

In practice, because of measuring and implementation errors, there will be 
a difference between the commanded behavior and the actual behavior of the 
damper. We summarize these differences by introducing the distinction between the 
actual nominal velocity, vq, and the commanded nominal velocity, denoted v 0 . 
Throughout the paper, the asterisk will denote measured or commanded quantities 
that differ from the actual ones because of the presence of error. 

The definition of the pre-image of a goal as the set of positions that can reach 
the goal with one commanded velocity can be retained for generalized damper 
motions. Under compliant motion, however, the moving object may reach the 
goal indirectly by sliding on intervening surfaces. Therefore, compliant motions 
typically produce larger pre-images than pure positioning motions. The increased 
pre-image indicates less sensitivity to uncertainty (compare Figures 8(a) and 8(b)). 

2.4. Friction 

A crucial consideration in the analysis and synthesis of fine-motion strategies 
is the effect of friction. A simple model of friction for planar motion without 
rotation is as follows. We assume that the objects are of a single material with 
equal coefficients of static and sliding friction, p. The reaction force from contact 
at a point on a surface will lie within a friction cone with apex at the point of 
/"N contact and center line along the surface normal. The angle between the normal 

and the sides of the cone is the friction angle, <j> — tan -1 p. If the applied force 
points into the friction cone, i.e., if the angle of the force vector to the surface 
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Figure 8. Pre-images for position control vs. generalized damper 



Figure 9. Friction cone 
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normal is less than <j>, then no motion will result. If the angle of the force vector to 
surface normal is greater than <j) sliding will result (see Figure 9). 

This model of friction can be extended to include rotations and moments, but 
the details are beyond the scope of this discussion (see [Erdmann 83]). In what 
follows, we need only assume that some nominal velocity vectors will cause sticking 
on a surface and others will cause sliding. We assume, furthermore, that the range 
of nominal velocity vectors that cause sticking for a surface can be conservatively 
bounded by a cone. The computation of pre-images must take into account the 
possibility of “sticking” on a surface. In particular, assuming the motion is generated 
by a damper (with B = 61), if the range of nominal velocities for the pre-image 
contains nominal velocities whose angle to the normal of some surface is less than 
the friction angle, <f>, then the motion will stop at that surface (see Figure 9 and 
the Appendix). 

2.5. Examples 

We now have the conceptual tools necessary to synthesize a strategy for the 
simple example of Figure 3. In this section we illustrate one particular approach 
to synthesis of strategies based on the use of pre-images. The method used in this 
section is a subset of the general approach described in Section 3. 

Our goal is to identify some sub-region P of the free space C and a command 
nominal velocity Vo such that P is the strong pre-image, under vo, of the goal 
surface. Equivalently, P must not overlap the weak pre-image of any surface where 
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motion along vo may stick or not reach the goal. We will use this constraint to 
drive a simultaneous search for P and Vo- 

The key problem is in discovering vo. Our approach here is to narrow in on 
feasible values of vo by progressive refinement. We start with the complete range 
of possible Vo’s and remove from that range any values that can possibly lead 
to failure (by sticking or not reaching the goal). At each step of the algorithm, 
we compute the strong pre-image of the goal for the current range of Vo’s. The 
strong pre-image for a range of command velocities is the intersection of the strong 
pre-images for each of the velocities. These are the positions guaranteed to reach 
the goal for all the velocities in the range 2 . This is the same definition that we saw 
in section 2.2 for the strong pre-image in the presence of velocity error. In fact, as 
long as the velocity ranges used to compute pre-images are greater than 2e v , we 
need not concern ourselves further with velocity uncertainty. Once the algorithm 
has chosen a final velocity range, we can pick a specific velocity from the range 
such that all velocities within the velocity error fall in the chosen velocity range. 
Narrower velocity ranges will not yield such a safe velocity. 

For now we will ignore the need for backward chaining and sketch an algorithm 
for synthesizing single motions. We will deal with backward chaining presently. 
The basic algorithm steps are as follows: 

1. Compute P, the strong pre-image of the goal surface, for the current 
range of commanded velocities. If the current range of velocities is split 
into disjoint sub-ranges, then Steps 1 and 2 should be repeated for each 
sub-range (see Figure 10). 

2. If P includes an uncertainty ball centered at some starting position, then 
return P and the current velocity range. 

3. Pick i to be a surface (other than a goal) where the robot may “stick”, 
i.e. such that some velocity in the current velocity range points into the 
surface’s friction cone. If no such surface exists then stop. 

4. Remove from the range of commanded velocities any velocity pointing 
into the friction cone of x. 

5. Go to Step 1. 

We can illustrate the operation of this algorithm on our example as follows. 
Construct a directed graph with nodes for each of the surfaces in Figure 3 and one 
node representing free space ( C ). A link is directed from node m to node n in the 
graph if m and n are direct neighbors and m is in the weak pre-image of n for 
the specified velocity range. That is, there is a link from m to n if some velocity 
in the current range may cause the robot to move from some point in m (which is 
not in n ) to some point in n (which is allowed to be at the intersection of m and 
n) without going through points in any other node. In principle, the graph should 
have nodes representing the vertices; we have left them out for simplicity. This 

2 Note that the weak pre-image for a range of velocities is the union of the weak pre-images for 
velocities in the range, i.e., positions that may reach the goal for some velocity in the range. 
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simplification introduces the need for the phrases in parentheses. See, for example, 
the link between A and B and B and G (but not viceversa) in the first example. 
We will call this the reachability graph for that range of commanded velocities. 
The reachability graph plays a key role in algorithms for computing the strong 
pre-image of the goal. 

In our example, we start out with a range of commanded velocities including 
any velocity with a y component less than or equal to zero (we diagram ranges 
of commanded velocities as sectors of a circle). These are the velocities that will 
move p from nearby points onto the goal surface G. The reachability graph for 
this range of velocities is shown at the top of Figure 10. In this figure, we have 
indicated those surfaces where the moving object may stick (using the electrical 
ground symbol). The (potentially) sticking surfaces are those whose friction cones 
overlap the current velocity range. For simplicity, we assume that the contact on 
surfaces B and D are point contacts. 

Figure 10 illustrates the reachability graph and pre-image of the goal each 
time Step 1 is executed. The surfaces used to constrain the range of commanded 
velocities (Step 3) were chosen in the following order: B,D,A,E. The particular 
order does not affect the final result in this case. The algorithm terminates at the 
fourth cycle. In Figure 10(d), we have shown only one of the two velocity ranges 
(and corresponding P’s) that result from discarding velocities that may stick on A 
or E. The remaining velocity range leads to a pre-image that is the mirror image 
(about the hole axis) of the one in Figure 10(d). Any commanded velocity within 
either of these remaining velocity ranges will reach the goal from any position 
within P. Note that the single motion strategy developed by this approach is a 
biased search (see Section 1.1). This is a good choice since we have not included 
chamfers or rotation in our problem definition. 

The example above can be done with a single motion (because the friction 
cones of the horizontal and vertical surfaces do not overlap). We did not require the 
use of backward-chaining. After Step 2 of the algorithm we have a choice of refining 
the range of directions or of using the current P as the goal for a recursive call to the 
same algorithm. In principle, we can follow these two paths non-deterministically. 
In practice, this requires a search guided by considerations such as: the number of 
motions in the strategy thus far and the size of P. 

The example in Figure 11 illustrates the use of backward-chaining to develop 
multi-move strategies. In this example, we assume that the first four steps proceed 
essentially as in the example in Figure 10. The final pre-image of the first example 
now becomes G 1 , the goal for the next recursion level. The method applies as before 
and generates a new pre-image and velocity range. The strategy, then, consists of 
choosing some velocity from this i-ange, moving until transition into G 1 is detected, 
and changing the commanded velocity to one of those from the range obtained in 
the first example. 

We noted earlier that, for each commanded motion in a strategy, it is necessary 
to define a predicate which indicates that the goal has been reached. In multi-move 
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strategies, this condition signals that another motion should be commanded. Three 
types of basic termination conditions are available: 


1. Position termination: Terminate if the measured position of p is such 
that all possible actual positions consistent with the measurement are 
within the goal region. 

2. Velocity (force) termination: Terminate if the observed velocity of p is 
such that all possible actual velocities consistent with the measurement 
can only occur within the goal region. Note that since motions are 
generated by a generalized damper, the difference between actual velocity 
and commanded velocity provides information about reaction forces, e.g.,' 
contact with a surface. 


3. Time termination: Terminate if the elapsed time is such that all positions i 
consistent with the commanded motion and observed data are within the ( 
goal. , 


Position termination requires that all actual positions consistent with the 
measured position be within the goal. This is equivalent to the measured position 
being in the goal after shrinking it by e c along its boundary to free space. If any 
of the dimensions of the goal region are less than e c , then position feedback is not 
a reliable indicator of reaching the goal. When the goal is a surface, for example, 
shrinking will cause the goal region to vanish. In these cases, we must rely on 
velocity termination which requires that the observed velocities, e.g., when landing 
on or leaving from a surface, be unambiguous relative to surfaces that may be 
confused with the goal due to position measurement error. Time termination is also 
useful, when applicable, as it is much simpler to test than position termination. 

Velocity termination is the most useful termination condition when faced with, 
large position uncertainty. The strategy synthesized from the example in Figure 11, 
illustrate this. The first motion required by the strategy can be terminated when 
the x component of observed velocity is zero, i.e., when p strikes one of walls on the 
right of the hole. The second motion can be terminated when either the x and/or 
y components of the observed velocity are zero, i.e., when p is at the left bottom 
corner of the hole. 

These two examples illustrate the class of fine-motion strategies we wish to 
consider. The strategies operate over a wide range of uncertainty without explicitely 
computing where the parts are relative to each other. The strategies do not keep 
any explicit history of previous events although, we will see later, history is implicit- 
in the strategies. 


2.6. Configuration Space 


The basic operation in the ‘synthesis method described above is computing 
the strong pre-image of a goal. To do this, we first transform the input problem, 
involving a moving object and stationary obstacles, into an equivalent problem 
involving a point and transformed obstacles. This transformation has a number 
of advantages. One is that it enables us to represent the pre-images as areas in 
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Figure 12. C-space representations make motion constraints explicit 



Cartesian input 



C-surfaces 


the transformed space. The key advantage, however, is that this transformation 
makes the constraints on motion explicit. This is illustrated in Figure 12 where 
an upright peg and chamfered hole are shown to lead to transformed obstacles, 
similar to those of a chamfered peg and unchamfered hole and to those of a tilted 
peg and unchamfered hole. The transformation has served to make explicit the 
underlying similarity of motion constraints in these tasks. In fact, the transformation 
reduces tasks involving “arbitrary” geometric interactions between objects to the 
interactions possible between a point and a set of surfaces. 

We have in this paper limited ourselves to two-dimensional translation. It is 
possible, however, to extend the transformation approach to more general motions 
using the configuration space of a task [Arnold 80, Lozano-Perez 81, 83]. A 
configuration of an object is the set of parameters needed to completely specify 
the position of all points of the object. The configuration of a rigid two-dimensionaL 
object, for example, can be specified by two displacements and an angle, that of 
a rigid three-dimensional object by three displacements and three angles, and that 
of a robot arm by its joint angles. For concreteness, we will be dealing exclusively 
with cartesian configurations, e.g., ( x,y,9 ) for objects in the plane, and not joint 
angle configurations. The space of all possible configurations for an object is known 
as the configuration space (C-space) of that object. An object A is represented as a 
point in its C-space; the coordinates of that point are the configuration parameters 
of A. 

Stationary obstacles in the environment of a moving object A can be mapped 
into the configuration space of A. The resulting C-space obstacles are those 
configurations of A which would lead to collisions between A and the obstacles. 
Configurations on the surface of the C-space obstacle due to B are those where some 
surface of A is just touching a surface of B. If A and B are both three-dimensional 
polyhedra, the surfaces of the C-space obstacle for B arise from each of the 
feasible contacts between of vertices, edges, and faces of A and B (see Figure 13) 
[Lozano-Perez 83]. Therefore, each face of a C-space obstacle represents a particular 
type of geometric constraint on A. A range of positions (and orientations) of A can 
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Figure 13. Geometric conditions giving rise to C-surfaces 



Figure 14. Cross sections of peg-in-hole C-surfaces: (a) no chamfer (b) chamfer 





e 



be represented as a volume in the C-space of A and a motion of A is a curve in the 
C-space. 

As an illustration of the use of C-space surfaces, consider the familiar 
two-dimensional peg-in-hole problem from Figure 3. We can construct a three- 
dimensional C-space of (a :,y,0) configurations of the peg. In this space, the hole 
defines an obstacle (see Figure 14(a)). Note that although the resulting surfaces are 
curved, for each value of 9 the (x, y) cross section of the C-space surfaces is polygonal. 
The surfaces represent one-point contacts and the edges at the intersections of 
surfaces represent two-point contacts. Line-line contacts also give rise to edges at 
the intersections of one-point contact surfaces. Figure 14(b) shows cross sections 
for a peg and chamfered hole. 

/‘•■s The C-space representation can be extended to more general kinematic 

situations. In general, motion subject to geometric and kinematic constraints can 
be defined as collections of equalities and inequalities that must hold among the 
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parameters that determine the configurations of the robot and the objects in the 
task. These inequalities represent C-surfaces [Mason 81]. Take the constraint that 
a robot hand remain in contact with a crank handle as it rotates. The constraint 
relating the position of the hand, ( x , y ), to the position of the crank (a constant) 
and its current angle, a, is a curve (one-dimensional surface) in the configuration 
space of the task, i.e., the (x, y, a) space. 

Our goal is to make the detailed analysis of assembly operations algorithmic by 
casting it in terms of C-surfaces. The purely geometric aspects of the analysis have 
been exploited in earlier work on obstacle avoidance [Brooks and Lozano-Perez 
83, Lozano-Perez 81, 83]. C-surfaces also share many of the characteristics of 
“real” surfaces with respect to force analyses. This was exploited by Mason [81] to 
synthesize compliant motions. The synthesis approach described here also requires 
a mechanism for computing the effects of friction. Recent work has developed a 
definition of friction cones for C-surfaces [Erdmann 83]. Work is under way to show 
that conditions for avoiding jamming for the peg-in-hole can be re-stated in terms 
of the relationship of applied forces to these C-space friction cones. 

3. A General Framework 

In the previous section we illustrated an abstract planning algorithm for 
fine-motion strategies. Although that algorithm is representative of our approach 
f"*' to fine-motion synthesis, it is not the most general formulation of the approach. 

In particular, that algorithm embodies a restrictive assumption on the class of 
single-motion strategies. It only considers strategies obtained by discarding all 
velocity vectors that point into the friction cones of some subset of the task 
surfaces. In some cases, further restrictions of the class of velocity vectors would 
produce a better strategy. The algorithm of the previous section does not provide 
a mechanism for further restricting the range of velocities. More significantly, we 
have not provided a criterion for defining what a “better” strategy might be. 

In this section we will present a more general framework for our approach to 
fine-motion synthesis. Although the description of this approach takes the form 
of an algorithm, it is not detailed enough to be considered an effective procedure. 
Our goal here is to formulate the correctness conditions for a class of synthesis 
algorithms. This framework can be used to elucidate to what extent particular 
synthesis methods (for the class of fine-motion strategies we are considering) are 
“optimal.” In particular, we are interested in strategies that make the best possible 
use of sensory data. 

Development of the general framework begins with a description of the form 
of termination predicates for motions, followed by a discussion of the pre-image 
definition, and of the necessity of passing multiple subgoals to recursive calls of the 
planner. The rest of the section consists of a formal description of the framework, 
/""N expressed as an abstract algorithm, and an extended example of its application to 

the peg-in-hole problem of section 2. 
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3.1. Termination Predicates 

Much of the burden of interpreting uncertain information falls on the 
termination predicate, which must decide when the current goal has been achieved. 
It is obviously important that termination not be premature; otherwise subsequent 
motions will proceed on a false assumption. On the other hand, failure to 
terminate the motion when the goal is demonstrably attained is also bad; the 
missed opportunity could prevent successful completion of the task. Hence it is 
important that the termination predicate make the best possible use of the available 
information. 

One restriction is placed on the form of the termination predicates: we will 
exclude predicates which record sensory data for later use. The decision to terminate 
the motion must be made based on current sensor readings alone. As we shall 
see, there is another mechanism which encodes some history, so this constraint is 
not as debilitating as it may first appear. If later developments suggest that this 
restriction should be relaxed, the framework can be modified by allowing a state 
function to be defined along with each predicate. 

The form of the termination predicate will be introduced first with the 
assumption of perfect sensing and control. Consider the situation just after a 
command has been issued. Given perfect knowledge of the initial position and a 
perfect controller, and assuming good dynamic models, the planner could predict 
the subsequent trajectory of the robot. If the position and force sensors were perfect, 
it would be a simple matter to watch the sensors, or the time, and halt the motion 
when the robot reaches a goal. 

To address more realistic problems, we will first relax the assumption of perfect 
sensing. The planner still knows what trajectory the robot will follow, but the 
sensing information cannot be taken at face value. It is necessary to construct an 
interpretation of the sensory data, which will be the set of all positions/velocities 
consistent with the sensory data and with the trajectory. Once this is accomplished, 
termination is again simple—if this interpretation of the sensory data is a subset 
of a goal, the motion is terminated. 

The final step is to relax the assumption of perfect control and known initial 
position. Suppose we have a set of possible initial positions, and a set of possible 
nominal velocities. Each different combination of initial position and nominal 
velocity will give a different robot trajectory. Without knowing which trajectory is 
the “real” one, the predicate must terminate the motion with a guarantee of being 
in a goal. To see how this is done, imagine that there is a different robot for each 
trajectory, i.e. that all of the trajectories are being executed simultaneously. For 
each robot, we can apply the procedure of the previous paragraph: form the set 
of positions/velocities consistent with sensory data, intersect with the trajectory, 
check for inclusion in a goal. If the robots all agree that a goal has been achieved, 
the motion is terminated. This approach guarantees that for any initial position 
and nominal velocity consistent with the robot’s information, and for any position 
and velocity consistent with observations, termination will occur only if a goal is 
attained. 
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Figure 15. Some history is required to proceed into the hole 



When the termination predicate is constructed, it is important to bring all 
possible information to bear, so that the set of “virtual robots” may be made as 
small as possible. Thus far we have concentrated on the information encoded in 
the sensory data, but there is another important source of information. When we 
formulate a subgoal R and call the planner recursively, there are two important 
effects. First, the robot will be moved to R, But second, and more to the point, 
is that when the recursive call returns and the motion is executed, the planner 
knows that the robot is in R. To illustrate the distinction, consider a robot lightly 
touching a vertical wall, and suppose that the subgoal R is the wall. Although the 
robot is in R, this fact might not be apparent to the robot if the contact force is 
small and if the position sensors are noisy. Hence the planner is called recursively 
to “move” the robot to the wall. Presumably the planner will plan a horizontal 
motion into the wall. When the motion command is executed the robot will not 
move, which the termination predicate will interpret as evidence that the “motion” 
was successful. When the recursive invocation of the planner returns, it will have 
accomplished its mission, even though it did not move the robot at all. 

Another, more familiar, example illustrates the use of this information to 
construct the termination predicate. Suppose the planner is applied to the point- 
in-hole problem, with the position sensor giving a position at the lip of the hole 
(Figure 15). Using the position sensor alone, the planner would have to admit 
the possibility of the robot being positioned anywhere inside the disc centered on 
the sensed position. To attain the goal with a single motion would be impossible. 
However, if the accomplished subgoal R is also consulted, the set of possible initial 
positions is reduced—the robot must be in the intersection of R with the disc. 
Starting from this smaller set of possible initial positions, with a command nominal 
velocity down to the right, it is easy to confirm that all the virtual robots will 
achieve the goal. 

Thus the history of the robot, represented by the accomplished subgoal R, 
must be taken into account to construct the termination predicate. When the set 
of feasible trajectories is constructed, initial positions outside the subgoal R should 
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be excluded. That the termination predicate is dependent on the accomplished 
subgoal R is an important observation, which profoundly affects the ultimate form 
of the planning algorithm. 

3.2. Definition of Pre-image 

The fundamental element in our approach to planning is the ability to construct 
a pre-image: a set of points from which the goal can be attained in a single motion. 
In section 2, the pre-image depended on the goal G and a range of command 
nominal velocities. By proceeding more formally in this section, we find that the 
pre-image need not depend on the command nominal velocities, but that it does 
depend on the accomplished subgoal R. 

Conceptually, we can approach this problem as follows: For goal G, and for 
every possible observed initial position c init and accomplished subgoal R, construct 
the set S(c* nit , R, G) of all command nominal velocities such that the termination 
predicate, constructed as in section 3.1, is guaranteed to terminate the motion. If 
S(c* nit ,R,G ) is empty, there is no single motion that can be guaranteed to work 
for accomplished subgoal R and observed initial position c init . If S(c init ,R,G) is 
non-empty, then any element of S(c* nit , R,G) is sufficient to attain the goal. Now 
if the actual initial position of the manipulator is c init , the observed initial position 
c* nit could be anywhere in the sphere B{ci n n ) centered on Ci n a with radius equal 
to the tolerance on the position sensor. Ci n n should be in the pre-image if and only 
if every possible c* nit gives a non-empty S(c* nit ,R, G). Hence we can define the 
pre-image Pr{G) of a goal G: 

Pr{G ) = { Ci n it G R | V ^init G B{ c init)> S{ c init’ G) ^ 0}. 

The subscript R is used'as a reminder that the pre-image depends on R. Note also 
that the definition of Pr{G) excludes points outside R. To have a point in Pr(G) 
but not in R wouldn’t make much sense—such a point would be a good place for 
the robot to be, provided that it is somewhere else! 

3.3. Recursive Calls and Multiple Goals 

When the planner is first called, the robot could be anywhere in configuration 
space C. If the set of strategies guaranteed to attain the goal G from any point 
in configuration space S(c* nit , C, G) is non-empty, then the planner can choose 
and execute one of these strategies. If S(c* nit , C, G) is empty, the planner must 
construct suitable subgoals and initiate a recursive call to the planner to achieve 
these subgoals. 

Clearly, the planner should specify as subgoals only those sets from which it 
can achieve the goal, otherwise a recursive call will serve no purpose. To characterize 
these subgoals more precisely, let us look ahead a bit, and imagine that the recursive 
call to the planner has just returned. The recursive call guarantees that the the 
robot’s position is now in R. Thus the planner must plan a single motion, from initial 
position Ci n n in Bi which attains the goal G. By construction of the pre-image, 
such a motion exists only if c tn a is in the pre-image Pr(G). This observation serves 
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to define suitable subgoals —R should include only those points which are also in 
Pr(G), i.e. R C Pr{G). Since, by construction, R D Pr(G), we can restate this 
observation: R is a suitable subgoal if and only if R = Pr(G). 

Thus, any set satisfying the equation R — Pr{G) is a suitable subgoal. In 
general, there are a multitude of sets which satisfy this equation. For instance, if R 
satisfies the equation, so does any subset of R. The question is what to do with this 
multitude of subgoals. Do we pass them to recursive calls one at a time? Needless 
to say, the branching factor in this search can be rather large. However, another 
issue takes precedence. Situations occur for which the planner can be certain to 
attain one of two goals, but cannot be certain in advance which of the two goals 
it will attain. If the planner were passed either one of the two goals individually, it 
would fail to find a predicate guaranteed to terminate the motion. With both goals 
in hand simultaneously, it can plan a motion with confidence that it will ultimately 
be able to report which of the goals was attained. Hence we will pass all subgoals to 
the recursive call. This suggests that the approach be implemented without search, 
but we are not certain whether such an implementation will be possible. 

Since the planner will be passed multiple goals rather than a single goal, some 
adjustment of the notation is required. The set of goals will be written {£7 a }, the 
set of strategies guaranteed to attain one of the goals for given observed initial 
position c* nit and accomplished subgoal R will be written S(c init ,R, {G a }), and the 
pre-image will be written PR({G a })- 

3.4. A Formal Statement of the Framework. 

3.4.1. Nomenclature 
c configuration 

Ci n i t configuration at beginning of a motion 

v velocity 

t>o nominal velocity 

c* observed configuration 

c init observed configuration at beginning of a motion 
v* observed velocity 
i»q commanded nominal velocity 
t time 

C C-space, i.e., the set of all configurations 

B(c ) the “uncertainty ball” of configurations; i.e., the set of all configurations whose 
distance from c is within the tolerance of the position sensor. 

B[v) the “uncertainty ball” of velocities. 

B(v o) the “uncertainty ball” of nominal velocities. 
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{G a } current goal set. We wish to move the robot to one of the goals and return 
the identity of the goal. 

p(c*, v*, t) the termination predicate. For each goal in {G Q } it returns one of: “BUG”, 
indicating that no possible trajectory is consistent with any interpretation of the 
sensory data; “CONTINUE”, indicating that at least one possible trajectory 
exists, consistent with the sensory data not just at the goal; or “WIN”, 
indicating that all possible trajectories consistent with the sensory data are in 
the goal. 

S(c* nit ,R,{G a }) is the set {(v* 0 ,p(c* ,v* ,t)) \ p terminates}. By construction of the 
predicates, guaranteed'termination implies guaranteed attainment of a goal. 
So for a given observed initial configuration and accomplished subgoal R, this 
gives the set of all winning strategies, where a strategy comprises a command 
nominal velocity and a termination predicate. 

-PrKGcJ) is the pre-image {c init £ R | V c* nit £ B(c init ), S(c* nit , R, {G a }) 0}. 

{Rp} The sets of configurations R such that the pre-image Pjz({G a }) includes all 
of R, i.e., Pr({Gcx}) = R. This is the subgoal set—satisfaction of an element 
of this set by a recursive call will allow us to satisfy the current goal set. 

R the subgoal attained by recursive call to the planner. 

MotorCommand(u 0 ) execution of this program statement transmits the commanded 
nominal velocity to the controller, causing the manipulator to execute the 
planned generalized damper strategy. 

) the actual trajectory; it returns (c, v) - the actual configuration and velocity 
at time t - for initial position and nominal velocity uq,;- 

3.4.2. Algorithm 

Procedure FM({G a }) . 

Compute {i?^} 

If C is in {Rp} 

Then R <- C 
Else R <- FM({Rp}) 

(uj, p) <- choose (S(c* nit , R, {G a })) 
t <- 0 

MotorCommand(u 0 ) 

L {V„} <- p(cV, t) 

ForEach a Do If V a = BUG Then Error 
ForEach a Do If y a = WIN Then Return (G a ) 
Increment t 
Go L 
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Figure 16. Task illustrating construction of p. 



3.5. Example 

This section applies the algorithm to the two-dimensional peg-in-hole problem 
(Figure 16). FM is called with an initial goal set containing the single element G’. 
the bottom of the hole. Let Dij[t ) denote an actual trajectory; it returns (c, v ) - the 
actual configuration and velocity at time t - for initial position and nominal 
velocity vqj. 

First we construct an example illustrating construction of the termination 
predicate p(c ,v ,t). Such a predicate must be constructed for each (c init ,VQ,R). 
Here is the predicate for {c init)2 , v 0>2 , Ft), assuming that R includes B{c* nit2 ). 

Procedure P 2 , 2 {c*,v*,t) 

Flag <- False 

For all (c initti , v 0>j ) £ B{c* nit2 ) X B{v*^ 2 ) 

(c,u) <- D ii:i {t) 

If (c,u) £ B[c*) X B(v*) 

Then II c £ G 

Then Flag <- True 
Else Return(CONTINUE) 

If Flag Then Return(WIN) 

Else Return(BUG) 

S(c* nit , R, G) is the set of all (t’o,p) which are guaranteed to win if executed at a 
point in B{c init ) fl R■ For our example point, this set is empty. For example, the 
command nominal velocity v Q 2 is not in S{c* nit ,R,G) because trajectories from the 
left and right edges of B[c init ) will never reach the goal. 

The example predicate will give the behavior specified in sections 3.1 and 3.4, 
and is therefore “correct”. However, the form of the predicates is not completely 
satisfactory. The test for goal attainment is done simply by testing whether all 
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Figure 17. Singleton R's 



possible predicted trajectories have attained the goal. This satisfies the requirements 
of the formalism because of the way control error is modeled—we have assumed 
that the control error is constant during a trajectory. A more realistic model of 
error would yield more robust predicates, combining position, velocity, and time 
information to detect presence at the goal. 

The first step in the algorithm is to compute a' set of subgoals {Rp}. Recall 
that each element R of {Rp} is a set giving a pre-image Pfi({G a }) which is equal 
to R. The simplest way to begin is to construct the sets consisting of a single 
configuration. Such a set R = {c} is valid if and only if it gives a pre-image 
P {c >({G q }) equal to {c}. Suppose the recursive call reports the manipulator is at c, 
then the question is whether a single motion command can move the manipulator 
to the goal G. This is possible for all configurations c in the shaded region of Figure 
17. This region is the union of two half-planes and a circular disk [Turk 1983]. A 
point in one of the half-planes, such as C3, can move to the hole by selecting a 
velocity which is guaranteed either to fall to one side of the hole and slide in, or hit 
the hole directly. A point in the circular region, such as c 4 , can move to the hole 
by selecting a velocity which is guaranteed to hit the hole directly. 

Any set R satisfying R = Pfi({G a }) must be a subset of the region indicated 
in Figure 17. However, the shaded region does not itself constitute a good R. For 
instance, as in the earlier example, the planner might be unable to tell whether 
the robot is to the left or the right of the hole. Hence we must look for subsets 
R of the shaded region, each of which is equal to the corresponding pre-image 
,Pfj;({(j a }). Three different such subsets are shown in Figures 18(a) through 18(c). 
As an example, consider the set shown in Figure 18(c). There are three different 
regions in this set: one region to the left of the hole, one region to the right of 
the hole, and one region in the hole. We can demonstrate that this R is equal to 
the corresponding ^({Gq}) as follows: Suppose that a recursive call has reported 
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that the manipulator is in R. We now consult the position sensor. If c init is to the 
right of center, the manipulator cannot be in the left region, and the left-sliding 
command shown for point C 3 in Figure 17 will work. Similarly if c init is to the left of 
center, a right-sliding motion will work. This set works because any “incompatible” 
subsets—the left and right regions in this case—are separated by a distance of at 
least 2 e c . 

The sets shown in Figure 18 are maximal —they are not subsets of any other 
subgoals. Since the subset of any valid subgoal is itself a valid subgoal, it would 
make sense to pass only the maximal sets to the recursive call. However, situations 
do occur for which maximal sets do not exist, so we will simply pass all valid 
subgoals to the recursive call. 

Once the subgoal set {Rp} has been determined there is a recursive call to 
FM. As in section 2 . 1 , we will use a superscript numeral to indicate the “recursion 
level”. Thus we write that the recursive call’s goal set is the original call’s subgoal 
set by writing {G*} <- {Rp}- Construction of a predicate for a multiple goal set 
is a simple variation of the predicate constructed earlier. 

Procedure pi,i(c*, v*, t) 

Flag ’<- FALSE 

For All a Win a <- TRUE 

For All {c inii p, v Qij ) £ {R fl B(c* nit>1 )) X B(v* 0il ) 

(c,v) <- D itj {t ) 

If (c,v) £ B(c*)X B(v*) 
then Flag <- TRUE 

For All a If c £ G a Then Win a <- FALSE 
If Flag Then If For Some a Win Q = TRUE 
Then Return(WIN) 

Else Return(CONTINUE) 

Else Return(BUG) 

Now the recursive call must construct subgoals. In this case, the set of all 
configurations C is a valid subgoal. No further recursion is necessary, because 
one of the goals can be attained from any configuration whatever using a single 
motion command. The recursive call may immediately choose and execute a motion 
command. When the predicate terminates the motion, the identity of the subgoal 
attained is returned to the original call, which then chooses and executes a motion 
command carrying the manipulator to the bottom of the hole. 


4. Conclusion 

This paper has presented a formal approach to the synthesis of a class of 
fine-motion strategies. The approach operates directly from geometric descriptions 
of the task and explicit bounds on errors in sensing and motion. The basic method 
is structured around the computation of the pre-image of a goal region, i.e, set of 
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configurations that can reach the goal using a single compliant motion. We saw 
that the presence of errors in motion and sensing gives rise to a number of difficult 
problems in specifying motions and in deciding on termination conditions. Further 
work is in progress. 

Beyond presenting a specific synthesis approach, the paper has attempted 

1 . to illustrate the usefulness of modeling compliant fine-motion strategies 
as generalized damper motions that “slide” on C-surfaces (corresponding 

to geometric constraints), and 1 ■ 1 

2 . to establish correctness conditions for fine-motion programs operating 
under error in sensing and control. 

Our approach to these issues provides the foundation for our synthesis method. 
Moreover, we hope it may be useful to human programmers engaged in fine-motion 
synthesis. ' 

The approach in this paper is part of an attempt to develop a unified approach 
to robot motion planning, spanning obstacle avoidance [Brooks and Lozano-Perez 
83, Lozano-Perez 81, 83], compliant motion [Mason 81], pushing [Mason 82], 
grasping [Lozano-Perez 81, Mason 82], and (now) fine-motion strategies. We believe 
that if sophisticated sensor-based motion strategies are to be routinely used iii 
robotics, the analysis and synthesis of these strategies cannot (or should not have 
to) be done by human programmers on a task-by-task basis. Moreover, we are in, 
need of a theoretical basis for the development of the programming and control 
mechanisms best suited for sensor-based motion. For these reasons, there is a vital 
need for a unified (preferably mechanizable) approach to analysis and synthesis of 
robot motion. This paper is a step towards this goal. 


Appendix: Compliance via Generalized Damping 

Generalized damping is a very simple and flexible mechanism for implementing 
active compliance 3 [Whitney 77]. The basic approach is to define the desired 
behavior of the robot by the following relation: 

f = B(v — vo) 

where f is the vector of forces acting on the moving object, vo is the nominal velocity 
vector, and v is the actual velocity vector. In general, f is a vector of six cartesian 
forces and torques and v and Vo are vectors of six linear and rotational velocities. 
In our examples here, we limit ourselves to forces and linear displacements in the 
plane. 

Allowing the damping matrix B to be an arbitrary matrix can produce 
unusual behavior. One popular example is to relate forces in the —x directions to 
displacements in the -j -y direction so that the robot will climb over obstacles. We 


3 See [Mason 83] for a discussion of generalized damping versus the generalized stiffness and 
hybrid control approaches to compliant motion. 
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Figure 19. Geometry for generalized damper analysis 




will, however, limit ourselves to simple damping matrices. In particular, we assume 
B to be the a diagonal matrix 61, with 6 > 0. Note that the damper equation is 
now simplified to 


f = 6(v — v 0 ) 

or alternatively 

, 1 , 
V = v 0 -f- -f 
0 


Consider an object controlled by a generalized damper with Vo (at an angle 
9 below horizontal) on a rigid surface whose normal points along the y axis (see 
Figure 19(a)). When the object strikes the surface, three possibilities exist: (1) 
the object slides to the right; (2) the object slides to the left; or (3) the object 
remains motionless. We can use Coulomb’s law to determine which of these three 
possibilities will occur. . 

First consider case (1); the object slides to the right, so the velocity v is 
horizontal. Coulomb’s law dictates that the contact force f will make an angle 
<j) = tan^ji with the surface normal. Using the damper equation in a simple 
construction in velocity space (Figure 19(b)) we see that the nominal velocity angle 
9 must be less than § — <j>. Case (2), with the object sliding to the left is quite 
similar and yields the constraint that 9 must be greater than f + (j>. 

Finally consider case (3); the object sticks, i.e. the velocity v is zero. Coulomb’s 
law gives a constraint on the force: 

— — <f> < a < — 

2 ~ “2 

where a is the angle which the force f makes with the horizontal. Again, the damper 
equation implies a corresponding constraint on 9: 

The analysis above yields constraints on the nominal velocity Vo given the motion 
of the object. We are also interested in the opposite: given the nominal velocity vq, 
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what will be the resultant motion. In the present analysis this is easily obtained. 
If the nominal velocity angle 9 is less than | — <f>, only case (1), right-sliding, 
can occur. If the nominal velocity angle 9 is greater than f + (j>, only case (2), 
left-sliding, can occur. If the nominal velocity angle 9 is in the interior of the friction 
cone, i.e. if 

then only case (3), sticking, can occur. The only ambiguous cases occur when 
9 — | -f- <j> or when 9 = f — <j). These cases are often referred to as “impending 
motion”. 

Thus the class of nominal velocities which give a desired motion on a given 
surface is easily characterized in terms of the friction cone of the surface, making 
the generalized damper an ideal control function for synthesis of fine motions. 
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